MultibodyPlant hinders writing user non-finite values to context #22594
Conversation
SeanCurtis-TRI
added
the
release notes: fix
There was a problem hiding this comment.
+(release notes: fix)
+@amcastro-tri for feature review, please.
Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers
SeanCurtis-TRI
force-pushed
the
PR_mbp_nan
branch
from
537430c to
4b6b862
Compare
SeanCurtis-TRI
changed the title
There was a problem hiding this comment.
@amcastro-tri just wanted to ping you on this.
Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers
There was a problem hiding this comment.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers
multibody/plant/multibody_plant.h line 2675 at r1 (raw file):
this->ValidateContext(context); DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities())); if constexpr (!std::is_same_v<T, symbolic::Expression>) {
There is no reason to if-constexpr here. The DRAKE_THROW_UNLESS already turns itself into a no-op when asked to check a symbolic assertion.
Ditto throughout.
--- a/multibody/plant/multibody_plant.h
+++ b/multibody/plant/multibody_plant.h
@@ -2672,12 +2672,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
const Eigen::Ref<const VectorX<T>>& q_v) const {
this->ValidateContext(context);
DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities()));
- if constexpr (!std::is_same_v<T, symbolic::Expression>) {
+ DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
using std::isfinite;
- DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
- return isfinite(t);
- }));
- }
+ return isfinite(t);
+ }));
internal_tree().GetMutablePositionsAndVelocities(context) = q_v;
}
multibody/plant/multibody_plant.cc line 1720 at r1 (raw file):
DRAKE_MBP_THROW_IF_NOT_FINALIZED(); DRAKE_THROW_UNLESS(q.size() == num_positions()); if constexpr (!std::is_same_v<T, symbolic::Expression>) {
The q here is already double-valued. There is no reason to opt-out for Expression.
Ditto for elsewhere when the q is not T-typed.
MultibodyPlant::SetFoo(context, vector): when writing user-provided configuration values (qs and vs) into the context, we throw if the input has non-finite values. Includes the following APIs: - SetPositions() - 3 overloads - SetPositionsAndVelocities() - 2 overloads - SetDefaultPositions() - 2 overloads - SetVelocities() - 3 overloads - SetVelocitiesInArray()
SeanCurtis-TRI
force-pushed
the
PR_mbp_nan
branch
from
4b6b862 to
8b819db
Compare
There was a problem hiding this comment.
@jwnimmer-tri given I've only got a couple of days left this week, could I prevail upon you to feature review this?
Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers
multibody/plant/multibody_plant.h line 2675 at r1 (raw file):
Previously, jwnimmer-tri (Jeremy Nimmer) wrote…
There is no reason to if-constexpr here. The DRAKE_THROW_UNLESS already turns itself into a no-op when asked to check a symbolic assertion.
Ditto throughout.
--- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -2672,12 +2672,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> { const Eigen::Ref<const VectorX<T>>& q_v) const { this->ValidateContext(context); DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities())); - if constexpr (!std::is_same_v<T, symbolic::Expression>) { + DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) { using std::isfinite; - DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) { - return isfinite(t); - })); - } + return isfinite(t); + })); internal_tree().GetMutablePositionsAndVelocities(context) = q_v; }
Done
|
How about @sherm1? I think feature review should be someone qualified to assess the performance regression hazard. |
There was a problem hiding this comment.
As he's platform review tomorrow, would you be willing to cover platform?
Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers
There was a problem hiding this comment.
-@amcastro-tri +@sherm1 for feature review.
Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers
|
Sure. |
There was a problem hiding this comment.
Thanks.
Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers
There was a problem hiding this comment.
Feature 
Regarding performance: There is a significant bump to the cost of these methods (maybe 30% ish(?) since there is additional copying and cache invalidation being done) but I think it is worth it for these user-facing APIs which I would not expect to be used in inner loops. I checked the integrators -- they work directly with the (unchecked) Context API, which is good since they set state frequently, and any internal code can dodge the top level API if needed.
Reviewed 1 of 3 files at r1, 2 of 2 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers
multibody/plant/multibody_plant.h line 2675 at r2 (raw file):
this->ValidateContext(context); DRAKE_THROW_UNLESS(q_v.size() == (num_positions() + num_velocities())); DRAKE_THROW_UNLESS(all_of(q_v, [](const T& t) {
minor: I'm concerned about the inscrutable error message this will generate for a relatively common user-facing error condition (esp. for PyDrake users). Since this is done repeatedly (possibly rule-of-3 dodge) anyway, why not make a little ThrowIfNonFinite(func, ...) helper that generates a good message and use that everywhere?
MultibodyPlant::SetFoo(context, vector): when writing user-provided configuration values (qs and vs) into the context, we throw if the input has NaNs.
Includes the following APIs:
This change is